/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-19 21:18:32
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-18 21:09:31
 * @FilePath: /esp32_motor/motor/algorithm /pid.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "pid.h"
#include "esp_timer.h"
#include "math_common.h"

typedef struct
{
    _iq24 p;
    _iq24 i;
    _iq24 d;
    _iq20 outputRamp;
    _iq24 limit;
    _iq24 errorPrev;   //!< 最后的跟踪误差值
    _iq24 outputPrev;  //!< 最后一个 pid 输出值
    _iq24 integralPrev;//!< 最后一个积分分量值
    _iq24 ts;
    int   _1_ts;
    _iq24 _0_5ts;
} PidObj;

static void PID_setPid(PidControl *control, _iq24 p, _iq24 i, _iq24 d)
{
    PidObj *priv = control->priv;

    /*赋值*/
    priv->p = p;
    priv->d = d;
    priv->i = i;
}

static _iq24 PID_getRefValue(PidControl *control, _iq24 errorValue)
{
    PidObj *priv = control->priv;

    // P环
    _iq24 proportional = _IQmpy(priv->p, errorValue);
    // Tustin 散点积分（I环）
    _iq24 integral = priv->integralPrev + _IQmpy(_IQmpy(priv->i, priv->_0_5ts), errorValue + priv->errorPrev);
    integral       = _IQsat(integral, priv->limit, -priv->limit);
    // D环（微分环节）
    _iq24 derivative = _IQmpy(priv->d, _IQmpyI32(errorValue - priv->errorPrev, priv->_1_ts));

    // 将P,I,D三环的计算值加起来
    _iq24 output = proportional + integral + derivative;
    output       = _IQsat(output, priv->limit, -priv->limit);

    if (priv->outputRamp > 0)
    {
        // 对PID的变化速率进行限制
        _iq20 output_rate = _IQ20mpyI32(_IQtoIQ20(output - priv->outputPrev), priv->_1_ts);
        if (output_rate > priv->outputRamp)
        {
            output = priv->outputPrev + _IQmpyIQX(priv->outputRamp, 20, priv->ts, 24);
        }
        else if (output_rate < -priv->outputRamp)
        {
            output = priv->outputPrev - _IQmpyIQX(priv->outputRamp, 20, priv->ts, 24);
        }
    }
    // 保存值（为了下一次循环）
    priv->integralPrev = integral;
    priv->outputPrev   = output;
    priv->errorPrev    = errorValue;
    return output;
}

void PID_delete(PidControl *control)
{
    PidObj *priv = NULL;

    if (!control)
    {
        return;
    }
    priv = control->priv;

    if (!priv)
    {
        free(priv);
    }
    free(control);
}

esp_err_t PID_create(PidControl **control, PidConfig cfg)
{
    PidObj     *priv = NULL;
    PidControl *con  = NULL;

    if (!control)
    {
        return ESP_FAIL;
    }

    con = calloc(1, sizeof(PidControl));
    if (!control)
    {
        return ESP_FAIL;
    }
    priv = calloc(1, sizeof(PidObj));
    if (!control)
    {
        goto free;
    }
    con->priv        = priv;
    priv->p          = cfg.p;
    priv->i          = cfg.i;
    priv->d          = cfg.d;
    priv->limit      = cfg.limit;
    priv->outputRamp = cfg.outputRamp;
    priv->ts         = cfg.ts;
    priv->_1_ts      = 1.F / _IQtoF(priv->ts);
    priv->_0_5ts     = _IQmpy(priv->ts, _half);
    /*函数*/
    con->getRefValue = PID_getRefValue;
    con->setPID      = PID_setPid;

    /*输出*/
    *control = con;

    return ESP_OK;


free:
    PID_delete(con);
    return ESP_FAIL;
}
